//
//  verificationCutTraj.cpp
//  map-matching
//
//  Created by Lv Xin on 16/5/10.
//  Copyright © 2016年 Lv Xin. All rights reserved.
//
//  Personal Homepage: www.davidlvxin.com

#include "verificationCutTraj.h"
#include "dijkstra.h"
#include "distance.h"
#include "TR_SIM.h"
#include <cmath>
#include <algorithm>

using namespace std;

extern std::vector< std::vector<EdgeDis> > candidateEdge;
extern std::list<Route*> candidateR;
extern std::list<Route*> candidateRAfterTreeCut;

CutTraj::CutTraj(bool u, Map* m, Trajectary& t) : Verification(u), traj(t), mapSet(m){
    int T = candidateEdge.size();
    shortestPath = new std::vector<int>** [T - 1];
    for(int i = 0; i < T - 1; i++){
        int sizeA = candidateEdge[i].size();
        shortestPath[i] = new std::vector<int>* [sizeA];
        for(int j = 0; j < sizeA; j++){
            int sizeB = candidateEdge[i + 1].size();
            shortestPath[i][j] = new std::vector<int> [sizeB];
        }
    }
    splitSize = 1;
    maxSimPoint = -INF;
}

CutTraj::~CutTraj(){
    int size = candidateEdge.size();
    for(int i = 0; i < size - 1; i++){
        int sizeA = candidateEdge[i].size();
        for(int j = 0; j < sizeA; j++){
            delete [] shortestPath[i][j];
        }
        delete [] shortestPath[i];
    }
    delete [] shortestPath;
    int sizeR = splitRoute.size();
    for(int i = 0; i < sizeR; i++){
        list<Route*>::iterator s;
        for(s = splitRoute[i].begin(); s != splitRoute[i].end(); s++){
            delete *s;
        }
        splitRoute[i].clear();
    }
}

int CutTraj::ArgOnePoint(int rank){
    double vecx1 = traj.tra[rank].pos.lat - traj.tra[rank - 1].pos.lat;
    double vecy1 = traj.tra[rank].pos.lng - traj.tra[rank - 1].pos.lng;
    double vecx2 = traj.tra[rank + 1].pos.lat - traj.tra[rank].pos.lat;
    double vecy2 = traj.tra[rank + 1].pos.lng - traj.tra[rank].pos.lng;
    double sqrt1 = sqrt(vecx1 * vecx1 + vecy1 * vecy1);
    double sqrt2 = sqrt(vecx2 * vecx2 + vecy1 * vecy2);
    double vec = vecx1 * vecx2 + vecy1 * vecy2;
    return rad(acos(vec / sqrt1 / sqrt2));
}

void CutTraj::generateSmallTrajRoute(int rank){
    list<Route*> afterCut;
    if(useStrategy){
        list<Route*>::iterator s;
        vector<string> str;
        int lastCutPoint = splitTarget[splitSize - 1];
        for(s = candidateRAfterTreeCut.begin(); s != candidateRAfterTreeCut.end(); s++){
            string temp = "";
            for(int j = lastCutPoint; j < rank; j++){
                temp += ((*s)->rank[j] + '0');
            }
            str.push_back(temp);
        }
        int len = str.size();
        sort(str.begin(), str.begin() + len);
        int diff = unique(str.begin(), str.begin() + len) - str.begin();
        for(int i = 0; i < diff; i++){
            Route* route;
            route->init(rank - lastCutPoint);
            for(int j = 0; j < rank - lastCutPoint; j++){
                route->rank[j] = str[i][j] - '0';
            }
            afterCut.push_back(route);
        }
        splitRoute.push_back(afterCut);
    }else{
        list<Route*>::iterator s;
        vector<string> str;
        int lastCutPoint = splitTarget[splitSize - 1];
        for(s = candidateR.begin(); s != candidateR.end(); s++){
            string temp = "";
            for(int j = lastCutPoint; j < rank; j++){
                temp += ((*s)->rank[j] + '0');
            }
            str.push_back(temp);
        }
        int len = str.size();
        sort(str.begin(), str.begin() + len);
        int diff = unique(str.begin(), str.begin() + len) - str.begin();
        for(int i = 0; i < diff; i++){
            Route* route;
            route->init(rank - lastCutPoint);
            for(int j = 0; j < rank - lastCutPoint; j++){
                route->rank[j] = str[i][j] - '0';
            }
            afterCut.push_back(route);
        }
        splitRoute.push_back(afterCut);
    }
    if(rank == candidateEdge.size()) return;
    splitSize++;
    splitTarget.push_back(rank);
}

void CutTraj::cutRoute(){
    splitTarget.push_back(0);
    int size = candidateEdge.size();
    for(int i = 1; i < size - 1; i++)
        if(ArgOnePoint(i) > ALPHA)
            generateSmallTrajRoute(i);
    generateSmallTrajRoute(size);
}

void CutTraj::computeSmallRouteMaxSim(int id, Route* route){
    int rank = splitTarget[id];
    int endTarget = id == splitSize - 1 ? candidateEdge.size() : splitTarget[id + 1];
    int size = endTarget - rank;
    GenerateTrajectory temp;
    int firstSize = candidateEdge[0 + rank][route->rank[0]].edge.size();
    DoublePos post;
    PosWithRank r;
    post.lat = candidateEdge[0 + rank][route->rank[0]].edge[0].beginLat;
    post.lng = candidateEdge[0 + rank][route->rank[0]].edge[0].beginLng;
    r.pos = post;
    r.rank = candidateEdge[0 + rank][route->rank[0]].edge[0].begin;
    temp.point.push_back(r);
    for(int i = 0; i < firstSize; i++){
        DoublePos pos;PosWithRank r;
        pos.lat = candidateEdge[0 + rank][route->rank[0]].edge[i].endLat;
        pos.lng = candidateEdge[0 + rank][route->rank[0]].edge[i].endLng;
        r.pos = pos; r.rank = candidateEdge[0][route->rank[0]].edge[i].end;
        temp.point.push_back(r);
    }
    for(int i = 1; i < size; i++){
        if(temp.point[temp.point.size() - 1].pos.lat != candidateEdge[i + rank][route->rank[i]].edge[0].beginLat || temp.point[temp.point.size() - 1].pos.lng != candidateEdge[i + rank][route->rank[i]].edge[0].beginLng){//如果需要最短路
            vector<int> path;
            path.clear();
            if(shortestPath[i + rank][route->rank[i - 1]][route -> rank[i]].size() == 0){
                path = findpath(temp.point[temp.point.size() - 1].rank, candidateEdge[i + rank][route->rank[i]].edge[0].begin);
                findpath(temp.point[temp.point.size() - 1].rank, candidateEdge[i + rank][route->rank[i]].edge[0].begin) = path;
            }else{
                path = shortestPath[i + rank][route->rank[i - 1]][route -> rank[i]];
            }
            int pathSize = path.size();
            for(int j = 0; j < pathSize; j++){
                DoublePos pp; PosWithRank rin;
                pp.lat = mapSet->node[path[j]].lat;
                pp.lng = mapSet->node[path[j]].lng;
                rin.pos = pp; rin.rank = mapSet->node[path[j]].rank;
                temp.point.push_back(rin);
            }
        }
        int RankSize = candidateEdge[i + rank][route->rank[i]].edge.size();
        DoublePos postt;
        PosWithRank rr;
        postt.lat = candidateEdge[i + rank][route->rank[i]].edge[0].beginLat;
        postt.lng = candidateEdge[i + rank][route->rank[i]].edge[0].beginLng;
        rr.pos = post;
        rr.rank = candidateEdge[i + rank][route->rank[i]].edge[0].begin;
        temp.point.push_back(rr);
        for(int j = 0; j < RankSize; j++){
            DoublePos poss;PosWithRank rs;
            poss.lat = candidateEdge[i + rank][route->rank[i]].edge[j].endLat;
            poss.lng = candidateEdge[i + rank][route->rank[i]].edge[j].endLng;
            rs.pos = poss; rs.rank = candidateEdge[i + rank][route->rank[i]].edge[j].end;
            temp.point.push_back(rs);
        }
    }
    TR_SIM sim(temp, traj, rank, endTarget);
    double dijSum = 0.0;
    for(int i = rank; i < endTarget; i++){
        dijSum += candidateEdge[i][route->rank[i]].dis;
    }
    sim.setPart1(1 - dijSum / (endTarget - rank));
    sim.computeSim();
    if(sim.getSim() > maxSmallSim[id]){
        maxSmallSim[id] = sim.getSim();
        maxSmallSimRoute[id] = temp;
    }
}

void CutTraj::combineAllSmallRoute(){
    int size0 = maxSmallSimRoute[0].point.size();
    for(int i = 0; i < size0; i++){
        maxSimRoute.point.push_back(maxSmallSimRoute[0].point[i]);
    }
    for(int i = 1; i < splitSize; i++){
        int begin = maxSmallSimRoute[i - 1].point[maxSmallSimRoute[i - 1].point.size() - 1].rank;
        int end = maxSmallSimRoute[i].point[0].rank;
        vector<int> path = findpath(begin, end);
        int pathSize = path.size() - 1;
        for(int j = 0; j < pathSize; j++){
            PosWithRank tmp;
            tmp.pos.lat = mapSet->node[path[j]].lat;
            tmp.pos.lng = mapSet->node[path[j]].lng;
            tmp.rank = mapSet->node[path[j]].rank;
            maxSimRoute.point.push_back(tmp);
        }
        int sizeNext = maxSmallSimRoute[i].point.size();
        for(int j = 0; j < sizeNext; j++){
            maxSimRoute.point.push_back(maxSmallSimRoute[i].point[j]);
        }
    }
    TR_SIM simFinal(maxSimRoute, traj);
    simFinal.computeSim();
    maxSimPoint = simFinal.getSim();
}

void CutTraj::computeMaxSim(){
    cutRoute();
    for(int i = 0; i < splitSize; i++){
        maxSmallSim.push_back(-INF);
        GenerateTrajectory t;
        maxSmallSimRoute.push_back(t);
    }
    for(int i = 0; i < splitSize; i++){
        list<Route*>::iterator s;
        for(s = splitRoute[i].begin(); s != splitRoute[i].end(); s++){
            computeSmallRouteMaxSim(i, *s);
        }
    }
    combineAllSmallRoute();
}

GenerateTrajectory CutTraj::getMaxSim(){
    return maxSimRoute;
}